classdef Tracer < handle
    %TRACE 该类主要完成追踪算法相关内容
    %   追踪算法内容主要包括基于修正的3/4逻辑法、CJPDA算法、PDA算法、卡尔曼滤波算法等
    
    properties
        tracks  % 用于存储成熟轨迹的结构体数组[track1, track2, ...]
        % 单个结构体其内容包括:
        % track.ID  该轨迹的编号
        % track.Class  车辆类别
        % track.points 该轨迹的迹,为nx18维的数据，
        % [1时间戳, 2id, 3x, 4y, 5vr, 6rcs, 7vx, 8vy, 9frame_num, 10Longitude, 
        % 11Latitude, 12Altitude, 13parking, 14retrograde, 15overspeed, 16D, 17x_diff, 18y_diff]
        % 其中的id是数据原始点云id,记忆帧此处为0，frame_num代表该点是来自第几帧,D代表离散程度或者面积x_diff, y_diff分别代表本帧散射点x最大差值和y最大差值
        % track.Xk_e 当前估计状态[x, vx, y, vy]
        % track.Pk_ee 该轨迹当前的估计误差协方差矩阵
        % track.life 轨迹生命值，初始值等于life，失败一次则减1，成功关联则重新赋值为life，当life值为0时，删除轨迹
        % memNum 记录该轨迹的记忆帧数量

        initTracks  % 初始轨迹，仅用于航迹起始,单个结构体其内容与tracks一致
        dead_tracks  % 用于保存已经死亡的轨迹,单个结构体其内容与tracks一致
        points  % 本次需要处理的点迹
        var_yr  % 测距y轴方差。
        var_xr  % 测距远距离方差
        var_vr  % 测速方差。
        q  % 过程噪声数量级
        id_index  % 车辆ID编号索引，由编号1开始至4294967296(2^32)循环,刚好到4字节无符号整型的上限
        time_now  % 当前帧时刻
        time_last  % 上一帧时刻
        frame_num  % 记录当前第几帧
        life  % 轨迹生命最大值，也就是最多记忆多少个点 
        L_door  % 位置波门0~1
        speed_door  % 速度波门0~1
        turn_v % 转弯最快速度(m/s),也就是一般为30km/h(8.33m/s)
        V_threshold % 车辆最大速度(一般为33.33m/s),也就是120km/h

    end
    
    methods
        function obj = Tracer(var_yr, var_xr,  var_vr, q, L_door, speed_door, turn_v, V_threshold, life)
            %TRACE 构造此类的实例parkings
            % var_nr: 测距精度单位(m),就是误差都会在这个范围内，可以理解为两个标准差95%范围内，可以认为标准差为accuracy_r/2，速度同理。
            % var_fr: 远距离方差
            % accuracy_v: 测速精度,单位(m/s),标准差为accuracy_v/2。
            % q: 过程噪声参数
            % L_door: 位置波门概率
            % speed_door: 速度波门概率
            % turn_v: 侧向位移速度
            % V_threshold: 纵向最大速度
            % life: 最大生命值
            obj.tracks = struct([]);  % 初始化结构体数组
            obj.initTracks = struct([]);
            obj.dead_tracks = struct([]);
            obj.var_yr = var_yr;
            obj.var_xr = var_xr;
            obj.var_vr = var_vr;
            obj.id_index = 1;
            obj.q = q;
            obj.L_door = L_door;
            obj.speed_door = speed_door;
            obj.turn_v = turn_v;
            obj.V_threshold = V_threshold;
            obj.life = life;
            obj.time_last = 0;
            obj.time_now = 0;
            obj.frame_num = 0;
        end
        

        function loadPoints(obj, points)
            % 载入点迹
            % points: 需要载入的点，为二维矩阵,单行格式为[时间戳, ID, x, y, v, rcs]
            obj.points = points;
            obj.time_last = obj.time_now;
            obj.time_now = points(1, 1);
            obj.frame_num = obj.frame_num+1;
        end


        function calculate(obj)
            % 轨迹关联计算
            % 判断是否有成熟轨迹，若有则进行轨迹关联,而后进行起始轨迹关联，然后剩余游离点用于轨迹起始
            dissPoints = obj.targetTraceAssociate(obj.points);
            dissPoints = initTrackAssociate(obj, dissPoints);  % 轨迹起始关联，剩余游离点用于轨迹起始头

            % 初始轨迹起始
            initNum = length(obj.initTracks);
            for i = 1:1:size(dissPoints, 1)
                % [时间戳, id, x, y, vr, rcs, vx, vy, frame_num, Longitude, Latitude, Altitude, parking, retrograde, overspeed, D, x_diff, y_diff]
                obj.initTracks(initNum+i).points = [dissPoints(i,1:6), 0, 0, obj.frame_num, 0, 0, 0, 0, 0, 0, dissPoints(i, 7:9)];  % vx,vy暂时不知道都填0，完成轨迹转换后再填入
                obj.initTracks(initNum+i).Xk_e = [];
                obj.initTracks(initNum+i).Pk_ee = [];
                obj.initTracks(initNum+i).ID = 0;  % 保存和正常轨迹一致
                obj.initTracks(initNum+i).life = obj.life;
                obj.initTracks(initNum+i).Class = 0;
                obj.initTracks(initNum+i).memNum = 0;
            end
        end


        function dissociatePoints = targetTraceAssociate(obj, points)
            % 成熟轨迹关联
            % return dissociatePoints: 完成关联后剩余的游离点

            dt = obj.time_now - obj.time_last;
            [useP, dissPI] = obj.calUsePAndDiss(obj.tracks, points);  % 计算那些轨迹应该用那些点
            dissociatePoints = obj.points(dissPI, :);

            tracksNum = length(obj.tracks);
            dead_I = zeros(1, tracksNum);  % 用于保存死亡轨迹的索引
            dead_Ilen = 0;
            for t = 1:1:tracksNum
                if useP(t) == 0  % 没有在波门内的点
                    Xk_e = obj.tracks(t).Xk_e;
                    Pk_ee = obj.tracks(t).Pk_ee;
                    [Xk_e, Pk_ee] = obj.predict(dt, Xk_e, Pk_ee, obj.q);

                    obj.tracks(t).Xk_e = Xk_e;
                    obj.tracks(t).Pk_ee = Pk_ee;
                    obj.tracks(t).life = obj.tracks(t).life - 1;
                    obj.tracks(t).memNum = obj.tracks(t).memNum + 1;  % 记忆帧+1
                    
                    P_last = obj.tracks(t).points(end,:);  % 使用上一阵的相关数据来填写
                    % [时间戳, id, x, y, vr, rcs, vx, vy, frame_num, Longitude, Latitude, Altitude, parking, retrograde, overspeed, D, x_diff, y_diff]
                    % 记忆帧原id填0
                    p = [obj.time_now, 0, Xk_e(1,1), Xk_e(3,1), P_last(5:6), Xk_e(2,1), Xk_e(4,1), obj.frame_num, 0, 0, 0, 0, 0, 0, P_last(16:18)];
                    obj.tracks(t).points = [obj.tracks(t).points; p];
                    if obj.tracks(t).life == 0
                        dead_I(dead_Ilen+1) = t;
                        dead_Ilen = dead_Ilen+1;
                    end
                else
                    Xk_e = obj.tracks(t).Xk_e;
                    Pk_ee = obj.tracks(t).Pk_ee;
                    point = points(useP(t), :);
                    [Xk_e, Pk_ee, ~, ~] = obj.klm(1, dt, Xk_e, Pk_ee, obj.q, point(3:4)');
                    obj.tracks(t).Xk_e = Xk_e;
                    obj.tracks(t).Pk_ee = Pk_ee;
                    % [时间戳, id, x, y, vr, rcs, vx, vy, frame_num, Longitude, Latitude, Altitude, parking, retrograde, overspeed, D, x_diff, y_diff]
%                     p = [point(1:6), Xk_e(2,1), Xk_e(4,1), obj.frame_num, 0, 0, 0, 0, 0, 0, point(7:9)];
                    p = [point(1:2), Xk_e(1,1), Xk_e(3,1), point(5:6), Xk_e(2,1), Xk_e(4,1), obj.frame_num, 0, 0, 0, 0, 0, 0, point(7:9)];
                    obj.tracks(t).points = [obj.tracks(t).points; p];
                    len = size(obj.tracks(t).points, 1);
                    obj.tracks(t).life = 3;
                    if len > 15 && abs(p(5)) > 4
                        obj.tracks(t).life = obj.life;
                    end
                    obj.tracks(t).memNum = 0;
                end
            end

            if dead_Ilen ~= 0
                dead_I = dead_I(1:dead_Ilen);
                obj.dead_tracks = [obj.dead_tracks, obj.tracks(dead_I)];
                obj.tracks(dead_I) = [];  % 删除死亡轨迹
            end
            
        end


        function [useP, dissPI] = calUsePAndDiss(obj, tracks, points)
            % 用于计算那一条轨迹该用哪一个点，以及用完剩余的游离点索引
            % tracks 轨迹
            % points 用于关联的点
            % return useP: 那一个轨迹该用哪一个点，如果useP(t)对应为0则说明该轨迹没有关联点
            % return dissPI: 游离点的索引
            dt = obj.time_now - obj.time_last;
            tracksNum = length(tracks);
            pointsNum = size(points, 1);
            % 波门内距离
            Ds = zeros(pointsNum, tracksNum) + 1e10;  % 用于保存各个点到我估计值的距离
            for i = 1:1:tracksNum  % 遍历每一条成熟轨迹
                endP = tracks(i).points(end, :);
                for j = 1:1:pointsNum  % 遍历每一个点
                    point = points(j, :);
                    Xk_e = tracks(i).Xk_e;
                    Pk_ee = tracks(i).Pk_ee;
                    [~, ~, Sk, Vk] = obj.klm(0, dt, Xk_e, Pk_ee, obj.q, point(3:4)');
                    D = Vk'/Sk*Vk;  %  两个自由度的卡方分布
                    if D < chi2inv(obj.L_door, 2)  % 位置波门
                        if abs(point(5) - endP(5)) < obj.speed_door
                            Ds(j, i) = Vk'*Vk;  %在波门内直接取距离（欧几里得距离）
                        end
                    end
                end
            end

            dissPI = zeros(1, pointsNum);
            dissPIlen = 0;
            for j = 1:1:pointsNum
                if all(Ds(j, :) == 1e10,"all")  % 这个点到所有目标距离都是无穷大，说明是游离点
                    dissPI(dissPIlen+1) = j;
                    dissPIlen = dissPIlen+1;
                end
            end
            dissPI = dissPI(1:dissPIlen);

            % 引入成熟度机制
            for t = 1:1:tracksNum
                if all(Ds(:, t) == 1e10, "all")  % 这个轨迹没有在波门内的点
                    continue;
                end
                [~, tI] = min(Ds(:, t));  % 当前目标最优点索引
                for i = t+1:1:tracksNum
                    if all(Ds(:, i) == 1e10, "all")  % 这个轨迹没有在波门内的点
                        continue;
                    end
                    [~, mI] = min(Ds(:, i));  % 当前目标最优点索引
                    if tI == mI  % 两着最优点相同，则看谁的轨迹长谁先用
                        multiple = size(tracks(t).points, 1)/size(tracks(i).points, 1);
                        if multiple > 4 || multiple<1/4
                            if (size(tracks(t).points, 1) - size(tracks(i).points, 1)) * (Ds(mI, t) - Ds(mI, i)) > 0   % 比你长并且比你距离长,或比你短距离比你短，需要对调距离值
%                                 fprintf('ID:%d ID:%d\n', tracks(t).ID, tracks(i).ID);
                                temp = Ds(mI, t);
                                Ds(mI, t) = Ds(mI, i);
                                Ds(mI, i) = temp;
                            end
                        end
                    end
                end
            end

            % 计算那一条轨迹该用哪一个点
            useP = zeros(1, tracksNum);
            for i = 1:1:tracksNum
                if ~all(Ds==1e10,'all')
                    [~, I] = min(Ds, [], 'all');
                    [j, t] = ind2sub(size(Ds), I);
                    useP(t) = j;  % 表示第t个目标使用第j个点
                    Ds(:, t) = 1e10;
                    Ds(j, :) = 1e10;  % 对应行列均改为极大值
                end
            end

        end


        function dissociatePoints = initTrackAssociate(obj, points)
            % 起始轨迹关联,各自选择波门内最优点进行关联
            % points: 目标追踪关联完成后剩余的游离点
            % return dissociatePoints: 完成关联后剩余的游离点

            tracksNum = length(obj.initTracks);
            if tracksNum == 0
                dissociatePoints = points;
                return;
            end
            pointsNum = size(points, 1);
            Ds = zeros(pointsNum, tracksNum) + 1e10;  % 用于保存每一个点到目标波门的距离最小为0，最大为1e10代表不在波门内
            for i = 1:1:tracksNum  % 初始轨迹遍历索引
                for j = 1:1:size(points, 1)
                    Ds(j, i) = obj.init_door(obj.initTracks(i), points(j, :));
                end
            end  

            dissPI = zeros(1, pointsNum);
            dissPIlen = 0;
            for j = 1:1:pointsNum
                if all(Ds(j, :) == 1e10,"all")  % 这个点到所有目标距离都是无穷大，说明是游离点
                    dissPI(dissPIlen+1) = j;
                    dissPIlen = dissPIlen+1;
                end
            end
            dissociatePoints = points(dissPI(1:dissPIlen),:);

            % 计算那一条轨迹该用哪一个点,距离最小的优先选，其他依次进行
            useP = zeros(1, tracksNum);
            for i = 1:1:tracksNum
                if ~all(Ds==1e10,'all')
                    [~, I] = min(Ds, [], 'all');
                    [j, t] = ind2sub(size(Ds), I);
                    useP(t) = j;  % 表示第t个目标使用第j个点
                    Ds(:, t) = 1e10;
                    Ds(j, :) = 1e10;  % 对应行列均改为极大值
                end
            end

            dt = obj.time_now - obj.time_last;
            for t = 1:1:tracksNum
                if useP(t) ~= 0  % 一旦关联成功则进行相关赋值操作
                    x1 = obj.initTracks(t).points(1, 3);
                    y1 = obj.initTracks(t).points(1, 4);
                    x2 = points(useP(t), 3);
                    y2 = points(useP(t), 4);
                    
                    Xk_e = [x2; (x2-x1)/dt; y2; (y2 - y1)/dt];
                    %  初始估计误差协方差矩阵
                    r = obj.var_yr;
                    Pk_ee = [r, r/dt, 0, 0;
                             r/dt, 2*r/(dt*dt), 0, 0;
                             0, 0, r, r/dt;
                             0, 0, r/dt, 2*r/(dt*dt)];
                    % [时间戳, id, x, y, vr, rcs, vx, vy, frame_num, Longitude, Latitude, Altitude, parking, retrograde, overspeed, cluster_D, x_diff, y_diff]
                    p = [points(useP(t), 1:6), Xk_e(2,1), Xk_e(4,1), obj.frame_num, 0, 0, 0, 0, 0, 0, points(useP(t), 7:9)];
                    obj.initTracks(t).points = [obj.initTracks(t).points; p];
                    obj.initTracks(t).Xk_e = Xk_e;
                    obj.initTracks(t).Pk_ee = Pk_ee;
                    obj.initTracks(t).life = 2;
                    obj.initTracks(t).ID = obj.id_index;  % 变成熟轨迹后需要有ID值
                    obj.id_index = obj.id_index + 1;

                    obj.initTracks(t).points(1, 7:8) = p(7:8);  % 补充第一个点的速度 
                end
            end

            mature_index = find(useP~=0);  % 关联成功的轨迹索引

            obj.tracks = [obj.tracks, obj.initTracks(mature_index)];
            obj.initTracks = [];  % 起始轨迹置空
        end


        function D = init_door(obj, initTrack, point)
            % 速度法建立初始波门,利用车辆最大侧向移动速度以及最大行驶速度建立初始位置波门
            % 又知道径向速度和位置信息则知道Vx,Vy呈线性关系,结合逻辑法利用速度建立波门。随后进一步利用径向速度筛选。
            % initTrack: 初始轨迹，这里实际上是只有一个点的轨迹
            % point: 要被判断是否在波门内的点
            % return D: 对应波门的距离
            r = obj.var_yr;
            point0 = initTrack.points(1,:);
            
            R = [obj.var_xr, 0; 0, r];
            P_unit = point0(3:4)/norm(point0(3:4));  % 单位化起始位置向量
            a = P_unit(1);  % 对应x
            b = P_unit(2);  % 对应y
            Vr = point0(5);  % 径向速度
            dt = obj.time_now - obj.time_last;

            D = 1e10;
            Dx = 1e10;  % 默认不在波门内用极大值代替距离
            Dy = 1e10;
            threshold = chi2inv(obj.L_door, 2); % 自由度为2的卡方分布，0.95概率对应的值

            % X轴向的波门
            Vy_extremum = [-obj.turn_v, obj.turn_v];
            Vx_extremum = -b/a * Vy_extremum + Vr/a;
            if ~(max(Vx_extremum) < -obj.V_threshold || min(Vx_extremum)>obj.V_threshold)
                Vx_extremum = sort([Vx_extremum, -obj.V_threshold, obj.V_threshold]);
                Vx_extremum = Vx_extremum(2:3);  % 取中间两个数，就是去掉极大极小了
                dx = max(0, point(3) - point0(3) - max(Vx_extremum)*dt) + max(0, -point(3) + point0(3) + min(Vx_extremum)*dt);
                dy = max(0, point(4) - point0(4) - max(Vy_extremum)*dt) + max(0, -point(4) + point0(4) + min(Vy_extremum)*dt);
                d = [dx, dy];
                dx = d/(R+R)*d';
                if dx < threshold  % 在范围内
                    Dx = dx;  % 在波门内换成对应的实际波门距离值
                end
            end
            

            % Y轴向的波门
            Vx_extremum = [-obj.turn_v, obj.turn_v];
            Vy_extremum = -a/b * Vx_extremum + Vr/b;
            if ~(max(Vy_extremum) < -obj.V_threshold || min(Vy_extremum)>obj.V_threshold)
                Vy_extremum = sort([Vy_extremum, -obj.V_threshold, obj.V_threshold]);
                Vy_extremum = Vy_extremum(2:3);  % 取中间两个数，就是去掉极大极小了
                dx = max(0, point(3) - point0(3) - max(Vx_extremum)*dt) + max(0, -point(3) + point0(3) + min(Vx_extremum)*dt);
                dy = max(0, point(4) - point0(4) - max(Vy_extremum)*dt) + max(0, -point(4) + point0(4) + min(Vy_extremum)*dt);
                d = [dx, dy];
                dy = d/(R+R)*d';
                if dy < threshold  % 在范围内
                    Dy = dy;  % 在波门内换成对应的实际波门距离值
                end
            end

            if Dx < threshold || Dy < threshold
                % 接下来进一步判断径向速度范围
                if abs(point(5) - point0(5)) < obj.speed_door
                    D = min([Dx, Dy]);
                end
            end
        end


        function [Xk_e, Pk_ee] = predict(~, dt, Xk_e, Pk_ee, q)
            % 当波门内无点的时候进行预测
            % dt: 状态转移矩阵中的时间差
            % Xk_e: 初始估计状态
            % Pk_ee: 初始状态，估计误差协方差矩阵
            % H: 量测矩阵
            % q: 过程噪声，数量级取10,1,0.1,0.01,0等值即可，自动填入对角线上。
            A = [1, dt, 0, 0;
                 0, 1, 0, 0;
                 0, 0, 1, dt;
                 0, 0, 0, 1];
            % 过程噪声
            Q = q*[1, 0, 0, 0;
                   0, 1, 0, 0;
                   0, 0, 1, 0;
                   0, 0, 0, 1];
            Xk_p = A*Xk_e;  % 下一个状态预测值
            Pk_pe = A*Pk_ee*A' + Q;  % 预测误差协方差矩阵

            Xk_e = Xk_p;
            Pk_ee = Pk_pe;
        end


        function [Xk_e, Pk_ee, Sk, Vk] = klm(obj, isEst, dt, Xk_e, Pk_ee, q, Zk)
            % 卡尔曼滤波,向量均为竖着的,仅限于本项目使用
            % isEst: 是否进行状态估计，0/不计算Xk_e和Pk_ee, 1/计算Xk_e和Pk_ee
            % dt: 状态转移矩阵中的时间差
            % Xk_e: 初始估计状态
            % Pk_ee: 初始状态，估计误差协方差矩阵
            % H: 量测矩阵
            % q: 过程噪声，数量级取10,1,0.1,0.01,0等值即可，自动填入对角线上。
            % r: 测量误差方差
            % Zk: 本次测量值

            % 状态转移矩阵
            A = [1, dt, 0, 0;
                 0, 1, 0, 0;
                 0, 0, 1, dt;
                 0, 0, 0, 1];
            % 过程噪声
            Q = q*[10, 0, 0, 0;
                   0, 10, 0, 0;
                   0, 0, 1, 0;
                   0, 0, 0, 1];
            % 量测矩阵
            H = [1, 0, 0, 0;
                 0, 0, 1, 0];
            % 量测误差协方差矩阵
            R = [obj.var_xr, 0;
                 0, obj.var_yr];
            Xk_p = A*Xk_e;  % 下一个状态预测值
            Pk_pe = A*Pk_ee*A' + Q;  % 预测误差协方差矩阵

            Zk_p = H*Xk_p;  % 量测预测
            Vk = Zk - Zk_p;  % 新息（量测残差）
            Sk = H*Pk_pe*H'+R;  % 新息协方差

            Kk = Pk_pe*H'/Sk;  % 卡尔曼增益

            if isEst
                Xk_e = Xk_p + Kk*Vk;
                Pk_ee = Pk_pe - Kk*H*Pk_pe;
            else
                Xk_e = [];
                Pk_ee = [];
            end
        end


        function finish(obj)
            % 最终结束，需要将还未死亡的轨迹合并到死亡轨迹中
            obj.dead_tracks = [obj.dead_tracks, obj.tracks]; % 因为帧结束了但是有的轨迹可能并没有死亡，所以要加进来
        end

%%---------------------------以上代码为追踪全部代码,以下下是分类识别事件判断代码及标准输出------------------------------%%
        function classify(obj)
            % 通过外部散射点区域以及RCS判断目标分类,默认为小汽车，主要不是摩托车和卡车就是小汽车
            % return 对已结束轨迹中的车辆进行分类，车辆类别,0-小汽车,1-货车,2-摩托车
            for t = 1:1:length(obj.dead_tracks)
                obj.dead_tracks(t).Class = 0;  % 默认为小汽车
                truckSizeSum = sum(obj.dead_tracks(t).points(:,17)>7.5);
                truckRcsMean = mean(obj.dead_tracks(t).points(:,6));
                if truckSizeSum >= 10 || truckRcsMean > 25
                    obj.dead_tracks(t).Class = 1;
                    continue;
                end

                motorRcsMean = mean(obj.dead_tracks(t).points(:,6));
                motorSizeSum = sum(obj.dead_tracks(t).points(:,17)>2.5);
                if motorSizeSum <= 4 && motorRcsMean <= 6
                    obj.dead_tracks(t).Class = 2;
                    continue;
                end
            end
        end


        function eventJudge(obj)
            % 超速和逆行事件判断
            % [1时间戳, 2id, 3x, 4y, 5vr, 6rcs, 7vx, 8vy, 9frame_num, 10Longitude, 11Latitude, 12Altitude, 13parking, 14retrograde, 15overspeed, 16D, 17x_diff, 18y_diff]
            for t = 1:1:length(obj.dead_tracks)
                for j = 1:1:size(obj.dead_tracks(t).points, 1)
                    if abs(obj.dead_tracks(t).points(j, 5))>16.667
                        obj.dead_tracks(t).points(j, 15) = 1;
                    else
                        obj.dead_tracks(t).points(j, 15) = 0;
                    end

                    if abs(obj.dead_tracks(t).points(j, 5)) < 0.2
                        obj.dead_tracks(t).points(j, 13) = 1;
                    else
                        obj.dead_tracks(t).points(j, 13) = 0;
                    end

                    obj.dead_tracks(t).points(j, 14) = 0;  % 直接设没有逆行
                end
            end
        end


        function outPs = criterionOutput(obj, minlen)
            % 用于基本的分类判断，事件判断以及标准输出
            % minlen: 输出最小长度，低于这个长度的不要
            % [1时间戳, 2id, 3x, 4y, 5vr, 6rcs, 7vx, 8vy, 9frame_num, 10Longitude, 11Latitude, 12Altitude, 13parking, 14retrograde, 15overspeed, 16D, 17x_diff, 18y_diff]
            outPs = [];
            look = [];
            for i = 1:1:length(obj.dead_tracks)
                ps = obj.dead_tracks(i).points;
                tracklen = size(ps, 1);
                if tracklen<minlen % 短轨迹直接不要
                    continue;
                end
                
                for j = 1:1:size(ps, 1)
                    vx = ps(j, 7);
                    vy = ps(j, 8);
                    outP = [ps(j, 1), obj.dead_tracks(i).ID, ps(j, 3:4), vx, vy, ps(j, 6),  obj.dead_tracks(i).Class, 0, 0 , 0, ps(j, 13), ps(j, 14), ps(j, 15)];
                    outPs = [outPs; outP];
                    ps(j, 2) = obj.dead_tracks(i).ID;
                    ps(j, 16) = obj.dead_tracks(i).Class;
                    look = [look; ps(j, :)];
                end
            end
%             [~, idx] = sort(outPs(:,1));
%             outPs = outPs(idx,:);
            writematrix(outPs,'ObjTrackData.csv');

            writematrix(look,'look.csv');
        end


    end
end

